#!usr/bin/python

#new protocal

import threading
import sys
from time import sleep, ctime
from queue import Queue
import time
import serial
from utils import *
import logger


#stop = [0xA5,0x07,0x00,0x02,0x00,0x00,0xA0]
#reset= [0xA5,0x07,0x00,0x04,0x00,0x00,0xA6]
scan = [0xA5,0x07,0x00,0x0C,0x00,0x00,0xAE]
force = [0xA5,0x07,0x00,0x0A,0x00,0x00,0xA8]
dbg = [0xA5,0x07,0x00,0x31,0x00,0x00,0x93]

mode = {'IDLE':0,'SCAN':1,'DBG':2}


class Lidar:
    
    CONST_FLAG_ERR =0x01
    CONST_FLAG_CAL = 0x02
    CONST_FLAG_COMM = 0x04
    
    def __init__(self, port):
        self.lidar_comm = serial.Serial()
        self.lidar_comm.port = port
        self.lidar_comm.baudrate = 115200
        self.lidar_comm.bytesize = 8
        self.lidar_comm.parity = 'N'
        self.lidar_comm.stopbits = 1
        self.lidar_comm.timeout = 1
        self.mode = mode['IDLE']  #???
        self.id = get_num_from_string(port)
        self.name = "lidar{0}".format(get_num_from_string(port))
        self.rxqueue = Queue(5*10214)
        self.dist_queue = Queue(5*1024)
        self.msg_len = 0
        self.msg_pos = 0
        self.flag = 0
        if self.lidar_comm.isOpen()==False:
            self.lidar_comm.open()
        if self.lidar_comm.isOpen()==True:
            print(self.lidar_comm.port+" is open!\n")
        else:
            print(self.lidar_comm.port+" can not open!\n")
            
        self.msgs=[]
        self.msg_pos = 0  #???
        
    
    def set_flag(self, flags):
        self.flag |= flags
    
    def get_flag(self, flags):
        return (self.flag&flags)
    
    def scan(self):
        global mode
        self.mode = mode['SCAN']
        self.flush()
        self.lidar_comm.write(scan)
        
    def force(self):
        self.mode = mode['SCAN']
        self.flush()
        self.lidar_comm.write(force)
    
    def reset(self):
        self.mode = mode['IDLE']
        self.lidar_comm.write(reset)
        
    def stop(self):
        self.mode = mode['IDLE']
        self.lidar_comm.write(stop)
        time.sleep(0.01)
        self.flush()

        
    def dbg(self):
        self.mode = mode['DBG']
        self.flush()
        self.lidar_comm.write(dbg)
        done = 0
        line =""
        local_time = time.time()
        while not done:
            line = self.lidar_comm.readline()
            line = line.decode('utf-8')
            print(line)
            if line=='':
                done=1
            if time.time()-local_time>3:
                return False
        
        return True
                
    def flush(self):  #self.flush()
        self.lidar_comm.flushInput()
        self.rxqueue.queue.clear()
        self.dist_queue.queue.clear()
        self.msg_len=0
        self.msg_pos = 0
        self.msgs=[]
        
    def quit_dbg(self):
        self.mode = mode['IDLE']
        
        quit_msg = "quit()\r\n".encode("utf-8")
        self.lidar_comm.flushInput()  #???
        self.lidar_comm.write(quit_msg)
        done = 0
        local_time = time.time()
        while not done:
            line = self.lidar_comm.readline()
            line = line.decode('utf-8')
            print(line)
            if line=='':  #返回空字符，代表指令执行成功???
                done=1
            if time.time()-local_time>1:
                return False
        return True
        
    def set_dist(self,dst):
        dst_cmd = "set dst({0})\r\n".format(dst)
        self.lidar_comm.write(dst_cmd.encode("utf-8"))
        done = 0
        line =""
        local_time = time.time()
        while not done:
            line = self.lidar_comm.readline()
            line = line.decode('utf-8')
            print(line)
            if line=='':
                done=1
            if time.time()-local_time>3:
                return False
        return True
        
    def readline(self,timeout=0):
        line = ""
        done = 0
        while not done:
            length = self.lidar_comm.inWaiting()
            if length>0:
                line = self.lidar_comm.readline()
                line = line.decode('utf-8')
            if line!="":
                done=1
        return line
    
        return self.lidar_comm.readline()
        
    def set_cal(self,level,gain,offset):
        self.flush()
        set_cal_msg = "set cal({0},{1:.6f},{2:.6f})\r\n".format(level,gain,offset)
        self.lidar_comm.write(set_cal_msg.encode("utf-8"))
        #line = self.lidar_comm.readline()

        #line = line.decode('utf-8')
        line = self.readline(2)  #???
        logger.debug(line)
        line = line[:-2]
        temp = line.split(':')
        data = temp[1].split(',')
        tgain = float(data[1])
        toffset = float(data[2])
        logger.info("level={0},gain={1},offset={2}".format(level,gain,offset))
        '''
        if tgain==gain and toffset==offset:
            logger.printf(logger.FOREGROUND_GREEN,"lidar{0} set cal pass".format(self.id))
            return True
        else:
            logger.error("lidar{0} set cal fail".format(self.id))
            return False
        '''
        
    def get_cal(self,level):
        self.flush()
        self.lidar_comm.write("get cal({0})\r\n".format(level))
        data = self.lidar_comm.readline()
        print(data)
    def read(self):
        if not self.dist_queue.empty():
            return self.dist_queue.get()
        else:
            return []
            
    def message_input(self):
        len = self.lidar_comm.inWaiting()
        if len>0:
            rx_data = self.lidar_comm.read(len)
            for i in range(len):
                self.rxqueue.put(rx_data[i])
    
    def message_parsing(self): 
        while not self.rxqueue.empty():
            data = self.rxqueue.get()
            if self.msg_pos==0:
                if data == 0xA5:
                    self.msgs=[]
            self.msgs.append(data)
            self.msg_pos += 1
            if self.msg_pos==3:
                self.msg_len=self.msgs[1]|(self.msgs[2]<<8)
            if self.msg_len!=0 and self.msg_pos==self.msg_len:
                checksum = 0
                #print("------msg_pos={0},msg_len={1},{2}\n".format(self.msg_pos,self.msg_len, len(self.msgs)))
                for i in range(self.msg_len-1):
                    checksum^=self.msgs[i]
                if checksum!=self.msgs[self.msg_len-1]:
                    print("------msg_pos={0},msg_len={1},{2}\n".format(self.msg_pos,self.msg_len, len(self.msgs)))
                    print(self.msgs)
                    self.msg_pos==0
                    self.msg_len=0
                    self.msgs=[]
                    checksum=0
                    print(">>>>checksum error")
                    continue
                cmd = self.msgs[3]
                payload_len = self.msgs[4]|(self.msgs[5]<<8)
                payload = self.msgs[6:self.msg_len-1]
                angle1=payload[0]|(payload[1]<<8)
                angleMax = payload[2]|(payload[3]<<8)
                dist=[]
                dist.append(angle1)
                dist.append(angleMax)
                dist.append(int(len(payload)/2)-2)
                for i in range(2,int(len(payload)/2)):
                    temp = payload[i*2]|(payload[i*2+1]<<8) 
                    dist.append(temp)
                print(dist)
                self.dist_queue.put(dist)
                self.msg=[]
                payload=[]
                self.msg_pos = 0
                self.msg_len = 0

    def rx_process(self):
        if self.mode==mode['DBG']:
            self.dbg_process()
        elif self.mode==mode['SCAN']:
            self.message_input()
            self.message_parsing()
        

def main():
    print("starting at:", ctime())


if __name__=='__main__':
    main()    
